#include <Servo.h>

#define SPEED_1      5 
#define DIR_1        4
#define SPEED_2      6
#define DIR_2        7
#define SERVO        9
#define PIEZO        8

#define INIT_ROT     90
#define MIN_REL_ROT  -60
#define MAX_REL_ROT  60

float current_rotation = INIT_ROT;
int current_rotation_dir = 0;
bool current_sound_state = 0;
Servo servo;

void setup(){
  Serial.begin(9600);
  for (int i = 4; i < 8; i++) {     
    pinMode(i, OUTPUT);
  }
  servo.attach(SERVO);
  servo.write(INIT_ROT);
  pinMode(PIEZO, OUTPUT);
}

void moveMotor(int id, int _speed){
  int _direction_ = 1;
  if(_speed < 0){
    _direction_ = 0;
  }
  int _speed_ = abs(_speed);
  
  if(id == 0){
    digitalWrite(DIR_1, _direction_);
    analogWrite(SPEED_1, _speed_);
  }else if(id == 1){
    digitalWrite(DIR_2, _direction_);
    analogWrite(SPEED_2, _speed_);
  }else if(id == 2){
    digitalWrite(DIR_1, _direction_);
    analogWrite(SPEED_1, _speed_);
    digitalWrite(DIR_2, _direction_);
    analogWrite(SPEED_2, _speed_);
  }
}

void rotateServo(float rotate){
  float new_rotate = rotate * current_rotation_dir;
  if(current_rotation + new_rotate <= INIT_ROT+MAX_REL_ROT && current_rotation + new_rotate >= INIT_ROT+MIN_REL_ROT){
    current_rotation += new_rotate;
    servo.write((int)current_rotation);
  }
}

void soundSignal(){
  current_sound_state = !current_sound_state;
  if(current_sound_state == 1){ digitalWrite(PIEZO, HIGH); }
  if(current_sound_state == 0){ digitalWrite(PIEZO, LOW); }
}

int getVals(String *func, int *vals, String data, char sep){
  bool neg = 0;
  int val_i = 0;
  for(int i=0; i<3; i++){
    (*func) += data[i];
  }
  for(int i=0; i<256; i++){ vals[i] = 0; }
  for(int i=3; i<data.length() && val_i < 256; i++){
    if(data[i] == '-'){ neg = 1; i++; }
    if(data[i] == sep){
      if(neg){ vals[val_i] = -vals[val_i]; }
      val_i++;
      neg = 0;
      continue;
    }
    vals[val_i] *= 10;
    vals[val_i] += (data[i]-0x30);
  }
  if(neg){ vals[val_i] = -vals[val_i]; }
  return val_i;
}

void loop(){
  rotateServo(0.005);

  if(Serial.available() > 0){
    String data = Serial.readStringUntil('\n');
    int vals[256];
    String operation; 
    int size = getVals(&operation, vals, data, ';');
    if(operation == "mov"){
      for(int i=0; i<(size+1); i+=2){
        moveMotor(vals[i], vals[i+1]);
      }
    }else if(operation == "rot"){
      current_rotation_dir = vals[0];
    }else if(operation == "sig"){
      soundSignal();
    }
  }
}
